#include "driver_pbs_03jn.h"

#define ASSERT(format,...) printf("Function %s Line %05d# "format"\n", __FUNCTION__, __LINE__,##__VA_ARGS__)

// Set INx, 1-HIGH  0-LOW
#define PBS03JN_SET_CH1_IN1(x) HAL_GPIO_WritePin(GPIOA,GPIO_PIN_4,(GPIO_PinState)(x))
#define PBS03JN_SET_CH1_IN2(x) HAL_GPIO_WritePin(GPIOA,GPIO_PIN_5,(GPIO_PinState)(x))
#define PBS03JN_SET_CH1_IN3(x) HAL_GPIO_WritePin(GPIOA,GPIO_PIN_6,(GPIO_PinState)(x))
#define PBS03JN_SET_CH1_IN4(x) HAL_GPIO_WritePin(GPIOA,GPIO_PIN_7,(GPIO_PinState)(x))

#define PBS03JN_SET_CH2_IN1(x) HAL_GPIO_WritePin(GPIOE,GPIO_PIN_8,(GPIO_PinState)(x))
#define PBS03JN_SET_CH2_IN2(x) HAL_GPIO_WritePin(GPIOE,GPIO_PIN_9,(GPIO_PinState)(x))
#define PBS03JN_SET_CH2_IN3(x) HAL_GPIO_WritePin(GPIOE,GPIO_PIN_10,(GPIO_PinState)(x))
#define PBS03JN_SET_CH2_IN4(x) HAL_GPIO_WritePin(GPIOE,GPIO_PIN_11,(GPIO_PinState)(x))


#define PBS03JN_GET_CH1_IN1() HAL_GPIO_ReadPin(GPIOC,GPIO_PIN_4)
#define PBS03JN_GET_CH1_IN2() HAL_GPIO_ReadPin(GPIOB,GPIO_PIN_0)
#define PBS03JN_GET_CH1_IN3() HAL_GPIO_ReadPin(GPIOB,GPIO_PIN_2)
#define PBS03JN_GET_CH1_IN4() HAL_GPIO_ReadPin(GPIOE,GPIO_PIN_7)

#define PBS03JN_GET_CH2_IN1() HAL_GPIO_ReadPin(GPIOE,GPIO_PIN_12)
#define PBS03JN_GET_CH2_IN2() HAL_GPIO_ReadPin(GPIOE,GPIO_PIN_13)
#define PBS03JN_GET_CH2_IN3() HAL_GPIO_ReadPin(GPIOE,GPIO_PIN_14)
#define PBS03JN_GET_CH2_IN4() HAL_GPIO_ReadPin(GPIOE,GPIO_PIN_15)

#define LOOP 7
unsigned char ReadInput1Loop(unsigned char channel)
{
  if(channel > 2 || channel < 1)
  {
     ASSERT("Invalid parameter! channel = %02d, which should be in [1,2]", channel);
  }
  int i  = 0;
  unsigned char sum = 0;
  for(i = 0; i < LOOP; i++)
  {
    if(channel == 1)
    {
      sum += PBS03JN_GET_CH1_IN1();
    }
    else
    {
      sum += PBS03JN_GET_CH2_IN1();
    }
  }
  return sum;
}


unsigned char ReadInput2Loop(unsigned char channel)
{
  if(channel > 2 || channel < 1)
  {
     ASSERT("Invalid parameter! channel = %02d, which should be in [1,2]", channel);
  }
  int i  = 0;
  unsigned char sum = 0;
  for(i = 0; i < LOOP; i++)
  {
    if(channel == 1)
    {
      sum += PBS03JN_GET_CH1_IN2();
    }
    else
    {
      sum += PBS03JN_GET_CH2_IN2();
    }
  }
  return sum;
}


unsigned char ReadInput3Loop(unsigned char channel)
{
  if(channel > 2 || channel < 1)
  {
     ASSERT("Invalid parameter! channel = %02d, which should be in [1,2]", channel);
  }
  int i  = 0;
  unsigned char sum = 0;
  for(i = 0; i < LOOP; i++)
  {
    if(channel == 1)
    {
      sum += PBS03JN_GET_CH1_IN3();
    }
    else
    {
      sum += PBS03JN_GET_CH2_IN3();
    }
  }
  return sum;
}


unsigned char ReadInput4Loop(unsigned char channel)
{
  if(channel > 2 || channel < 1)
  {
     ASSERT("Invalid parameter! channel = %02d, which should be in [1,2]", channel);
  }
  int i  = 0;
  unsigned char sum = 0;
  for(i = 0; i < LOOP; i++)
  {
    if(channel == 1)
    {
      sum += PBS03JN_GET_CH1_IN4();
    }
    else
    {
      sum += PBS03JN_GET_CH2_IN4();
    }
  }
  return sum;
}



void PBS03JN_SetIOValue(unsigned char channel, unsigned char value)
{
    unsigned char tmp = ~value;
    switch(channel)
    {
    case 1:
        PBS03JN_SET_CH1_IN1(tmp & 0x01);
        PBS03JN_SET_CH1_IN2((tmp & 0x02) >> 1);
        PBS03JN_SET_CH1_IN3((tmp & 0x04) >> 2);
        PBS03JN_SET_CH1_IN4((tmp & 0x08) >> 3);
        break;
    case 2:
        PBS03JN_SET_CH2_IN1(tmp & 0x01);
        PBS03JN_SET_CH2_IN2((tmp & 0x02) >> 1);
        PBS03JN_SET_CH2_IN3((tmp & 0x04) >> 2);
        PBS03JN_SET_CH2_IN4((tmp & 0x08) >> 3);
        break;
    }
}


void PBS03JN_SetArea(unsigned char channel, unsigned char area)
{
  if(area > 15)
  {
      ASSERT("Invalid parameter! area = %02d, which should be in [1,15]", area);
      return;   
  }
  if(channel > 2 || channel < 1)
  {
      ASSERT("Invalid parameter! channel = %02d, which should be in [1,2]", channel);
      return;   
  }
  PBS03JN_SetIOValue(channel,area);
}




// Get Current Output, return:
// 0 -No Obstacle detected & No Trouble
// 1 -Obstacle detected in current area, distance < farthest range
// 2 -Obstacle detected in current area, distance < middle   range
// 3 -Obstacle detected in current area, distance < nearest  range
// 4 -Trouble
unsigned int PBS03JN_GetOutput(unsigned char channel)
{
    unsigned int ret_val = 0;
    
    if (ReadInput4Loop(channel) > 4)
    {
        ret_val = PBS03JN_WARN_ERROR;
    }
    else if (ReadInput3Loop(channel) > 4)
    {
        ret_val = PBS03JN_WARN_RED;
    }
    else if (ReadInput2Loop(channel) > 4)
    {
        ret_val = PBS03JN_WARN_ORANGE;
    }
    else if (ReadInput1Loop(channel) > 4)
    {
        ret_val = PBS03JN_WARN_YELLOW;
    }
    else
    {
        ret_val = PBS03JN_NO_OBS;
    }    
    return ret_val;
}
